#!/usr/bin/env python
# encoding: utf-8


# 这份代码是进行伸手抓香蕉的备份代码


import sys
import time
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_srvs.srv import Empty
from std_msgs.msg import Int32MultiArray
from inspire_hand.srv import set_pos

class CartesianControl(object):
    def __init__(self):
        # 初始化节点
        super(CartesianControl, self).__init__()
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('cartesian_control')

        rospy.loginfo("初始化灵巧手控制模块")

        self.set_position_client = rospy.ServiceProxy('inspire_hand/set_pos', set_pos)
        rospy.loginfo("等待灵巧手位置设置服务就绪")

        try:
            # 检查是否存在夹持器
            self.is_gripper_present = rospy.get_param(rospy.get_namespace() + "is_gripper_present", False)
            if self.is_gripper_present:
                gripper_joint_names = rospy.get_param(rospy.get_namespace() + "gripper_joint_names", [])
                self.gripper_joint_name = gripper_joint_names[0]
            else:
                gripper_joint_name = ""
            # 获取自由度
            self.degrees_of_freedom = rospy.get_param(rospy.get_namespace() + "degrees_of_freedom", 7)

            # 创建 MoveItInterface 对象
            arm_group_name = "arm"
            self.robot = moveit_commander.RobotCommander("robot_description")
            self.arm_group = moveit_commander.MoveGroupCommander(arm_group_name, ns=rospy.get_namespace())
            rospy.loginfo("Initializing node in namespace " + rospy.get_namespace())
            
            # 订阅笛卡尔坐标系控制话题
            self.cartesian_subscriber = rospy.Subscriber("cartesian_control_topic", geometry_msgs.msg.Pose, self.cartesian_control_callback)

            self.is_init_success = True
        except Exception as e:
            print (e)
            self.is_init_success = False

    def cartesian_control_callback(self, pose_msg):
        # 处理笛卡尔坐标系控制命令的回调函数
        rospy.loginfo("Received Cartesian pose control command")
        self.reach_cartesian_pose(pose_msg)

    def reach_cartesian_pose(self, pose_msg):
        arm_group = self.arm_group

        # 设置目标笛卡尔姿态
        target_pose = geometry_msgs.msg.PoseStamped()
        target_pose.header.frame_id = "base_link"
        target_pose.pose = pose_msg

        # 规划并执行轨迹
        arm_group.set_pose_target(target_pose)
        arm_group.go(wait=True)

    def set_hand_position(self, grasp_angles_):
        try:
            # 将 Int32MultiArray 转换为 Python 列表
            grasp_angles = grasp_angles_.data

            # 调用服务代理函数，并传递参数列表
            response = self.set_position_client(*grasp_angles)
            if response.pos_accepted:
                rospy.loginfo("灵巧手位置设置成功!")
            else:
                rospy.logerr("灵巧手位置设置失败!")
        except rospy.ServiceException as e:
            rospy.logerr("调用服务失败: %s" % e)

    # def send_grasp_angles(self, grasp_angles_):
    #     pub = rospy.Publisher('grasp_angle', Int32MultiArray, queue_size=10)

    #     # 发布抓取角度消息
    #     pub.publish(grasp_angles_)
    #     rospy.loginfo("发布抓取角度消息: %s", grasp_angles_.data)

def main():
    cartesian_control = CartesianControl()

    if not cartesian_control.is_init_success:
        rospy.logerr("Initialization failed, exiting...")
        return

    rospy.loginfo("Cartesian control node initialized.")

    # 创建一个姿态消息 
    pose_msg = geometry_msgs.msg.Pose()

    # 设置消息的坐标 home
    pose_msg.position.x = 0.45611993071
    pose_msg.position.y = 0.00195518774071
    pose_msg.position.z = 0.434136790952

    pose_msg.orientation.x = 0.499762278048
    pose_msg.orientation.y = 0.499918626181
    pose_msg.orientation.z = 0.500809414448
    pose_msg.orientation.w = 0.499508721684

    # 调用函数并传递姿态消息作为参数
    cartesian_control.reach_cartesian_pose(pose_msg)

    # 设置消息的坐标 中间最上面 正对
    pose_msg.position.x = 0.458093492096
    pose_msg.position.y = -0.0277357604046
    pose_msg.position.z = 0.624325612844

    pose_msg.orientation.x = 0.499762278048
    pose_msg.orientation.y = 0.499918626181
    pose_msg.orientation.z = 0.500809414448
    pose_msg.orientation.w = 0.499508721684

    # 调用函数并传递姿态消息作为参数
    cartesian_control.reach_cartesian_pose(pose_msg)

    # 设置消息的坐标 中间最上面 香蕉点
    pose_msg.position.x = 0.656910152621
    pose_msg.position.y = 0.00131444628602
    pose_msg.position.z = 0.573762690417

    pose_msg.orientation.x = 0.499762278048
    pose_msg.orientation.y = 0.499918626181
    pose_msg.orientation.z = 0.500809414448
    pose_msg.orientation.w = 0.499508721684

    # 调用函数并传递姿态消息作为参数
    cartesian_control.reach_cartesian_pose(pose_msg)

    # 抓香蕉
    try:
        # 创建一个Int32MultiArray消息对象，并设置抓取角度值 香蕉
        grasp_angles = Int32MultiArray(data=[663, 706, 711, 618, 858, 1745])
        cartesian_control.set_hand_position(grasp_angles_ = grasp_angles)
    except rospy.ROSInterruptException:
        pass
    
    # 设置消息的坐标 中间最上面 正对
    pose_msg.position.x = 0.458093492096
    pose_msg.position.y = -0.0277357604046
    pose_msg.position.z = 0.624325612844

    pose_msg.orientation.x = 0.499762278048
    pose_msg.orientation.y = 0.499918626181
    pose_msg.orientation.z = 0.500809414448
    pose_msg.orientation.w = 0.499508721684

    # 调用函数并传递姿态消息作为参数
    cartesian_control.reach_cartesian_pose(pose_msg)

    # 设置消息的坐标 home
    pose_msg.position.x = 0.45611993071
    pose_msg.position.y = 0.00195518774071
    pose_msg.position.z = 0.434136790952

    pose_msg.orientation.x = 0.499762278048
    pose_msg.orientation.y = 0.499918626181
    pose_msg.orientation.z = 0.500809414448
    pose_msg.orientation.w = 0.499508721684

    # 调用函数并传递姿态消息作为参数
    cartesian_control.reach_cartesian_pose(pose_msg)

    # 释放
    try:
        # 创建一个Int32MultiArray消息对象，并设置抓取角度值 香蕉
        grasp_angles = Int32MultiArray(data=[0, 0, 0, 0, 0, 0])
        cartesian_control.set_hand_position(grasp_angles_ = grasp_angles)
    except rospy.ROSInterruptException:
        pass

    # 虚握
    try:
        # 创建一个Int32MultiArray消息对象，并设置抓取角度值 香蕉
        grasp_angles = Int32MultiArray(data=[73, 138, 135, 24, 1096, 1814])
        cartesian_control.set_hand_position(grasp_angles_ = grasp_angles)
    except rospy.ROSInterruptException:
        pass

    # rospy.spin()


if __name__ == '__main__':
    main()


